#include "rclcpp/rclcpp.hpp"
#include <string>
#include <vector>
#include <iostream>
#include "serial.hpp"
#include "config.hpp"
#include "motor_control.hpp"
class MotorDriverNode : public rclcpp::Node
{
public:
    MotorDriverNode() : Node("motor_driver_node")
    {
        
        // 声明参数，指定默认值
        declare_parameters();
    
        // 从参数服务器获取参数值
        get_parameters();    
            // 打印读取到的参数值
        RCLCPP_INFO(this->get_logger(), "成功读取参数:");
        if(run_mode==1){
                RCLCPP_INFO(this->get_logger(), "serial_port0: %s", motor_config.serial_port0_.c_str());
                RCLCPP_INFO(this->get_logger(), "baud_rate0: %d", motor_config.baud_rate0_);
                RCLCPP_INFO(this->get_logger(), "serial_port1: %s", motor_config.serial_port1_.c_str());
                RCLCPP_INFO(this->get_logger(), "baud_rate1: %d", motor_config.baud_rate1_);
                RCLCPP_INFO(this->get_logger(), "yaokong_channel: %d", motor_config.yaokong_channel);
                RCLCPP_INFO(this->get_logger(), "wheel_radius: %f", motor_config.wheel_radius);
                RCLCPP_INFO(this->get_logger(), "wheel_base: %f", motor_config.wheel_base);
                RCLCPP_INFO(this->get_logger(), "wheel_track: %f", motor_config.wheel_track);
                for (int i = 0; i < 8; ++i)
                {
                    RCLCPP_INFO(this->get_logger(), "motor%d_id: %x", i, motor_config.motor_drv[i].Id);
                    RCLCPP_INFO(this->get_logger(), "motor%d_delay: %d", i, motor_config.motor_drv[i].delay);
                    RCLCPP_INFO(this->get_logger(), "sensor%d_id: %x", i, motor_config.sensor_drv[i].Id);
                    RCLCPP_INFO(this->get_logger(), "sensor%d_delay: %d", i, motor_config.sensor_drv[i].delay);
                }
                for (int i = 0; i < 4; ++i)
                {
                    RCLCPP_INFO(this->get_logger(), "bianma%d_id: %x", i, motor_config.bianma_drv[i].Id);
                    RCLCPP_INFO(this->get_logger(), "bianma%d_delay: %d", i, motor_config.bianma_drv[i].delay);
                    RCLCPP_INFO(this->get_logger(), "bianma%d_zero: %d", i, motor_config.bianma_zero[i]);

                }
               
            }
        
        motor_control0_ = new MotorControl(motor_config.serial_port0_, motor_config.baud_rate0_);
        motor_control0_->setMotorControl(motor_config);
        timer_ = this->create_wall_timer(100ms, std::bind(&MotorDriverNode::timer_callback, this));
         
        
        
    }
private:
    void timer_callback()
    {
        // 定时器回调函数
       // motor_control0_->serial_.send("Hello Motor Driver");
        RCLCPP_INFO(this->get_logger(), "定时器触发1s");
       // motor_control0_->set_module_speed();
     //  motor_control0_->config_.yaokongline(); //测试用
        if(run_mode==0){
            motor_control0_->config_.yaokongline();
            motor_control0_->set_module_speed();
           
          //  RCLCPP_INFO(this->get_logger(), "遥控当前纵向速度=%d", motor_control0_->config_.yaokong_value.Left_vertic);
        }
    
        
        // 打印电机状态信息
       if(run_mode==1){
            
        RCLCPP_INFO(this->get_logger(), "电机接收到的消息数量1=%d  2=%d 3=%d 4=%d 5=%d 6=%d 7=%d 8=%d",
            motor_control0_->get_motor_rec_times(0),
            motor_control0_->get_motor_rec_times(1),
            motor_control0_->get_motor_rec_times(2),
            motor_control0_->get_motor_rec_times(3),
            motor_control0_->get_motor_rec_times(4),
            motor_control0_->get_motor_rec_times(5),
            motor_control0_->get_motor_rec_times(6),
            motor_control0_->get_motor_rec_times(7));
        RCLCPP_INFO(this->get_logger(), "遥控接收到的消息数量=%d", motor_control0_->get_yaokong_rec_times());
        RCLCPP_INFO(this->get_logger(), "编码器接收到的消息数量=%d %d %d %d",
            motor_control0_->get_bianma_rec_times(0),
            motor_control0_->get_bianma_rec_times(1),
            motor_control0_->get_bianma_rec_times(2),
            motor_control0_->get_bianma_rec_times(3));    
       }
        if(run_mode==2){
            RCLCPP_INFO(this->get_logger(), "当前速度度1=%d 2=%d 3=%d 4=%d 5=%d 6=%d 7=%d 8=%d",
                    motor_control0_->config_.motor_info[0].speed_cur,
                    motor_control0_->config_.motor_info[1].speed_cur,
                    motor_control0_->config_.motor_info[2].speed_cur,
                    motor_control0_->config_.motor_info[3].speed_cur,
                    motor_control0_->config_.motor_info[4].speed_cur,
                    motor_control0_->config_.motor_info[5].speed_cur,
                    motor_control0_->config_.motor_info[6].speed_cur,
                    motor_control0_->config_.motor_info[7].speed_cur
                );
            RCLCPP_INFO(this->get_logger(), "遥控当前速度度=%d %d %d %d %d %d %d",
                    motor_control0_->config_.yaokong_value.Left_horiz,
                    motor_control0_->config_.yaokong_value.Left_vertic,
                    motor_control0_->config_.yaokong_value.Right_horiz,
                    motor_control0_->config_.yaokong_value.Right_vertic,
                    motor_control0_->config_.yaokong_value.key1,
                    motor_control0_->config_.yaokong_value.key2,
                    motor_control0_->config_.yaokong_value.channel);
            RCLCPP_INFO(this->get_logger(), "编码器当前角度度1=%d 2=%d 3=%d 4=%d",
                    motor_control0_->config_.bianma_cur[0],
                    motor_control0_->config_.bianma_cur[1], 
                    motor_control0_->config_.bianma_cur[2],
                    motor_control0_->config_.bianma_cur[3]);
        }    
    }
    
    // 声明所有参数
    void declare_parameters() {
        this->declare_parameter<int>("run_mode", 0); //0=正常运行模式 1=查看接收到的消息数量 2=查看接收到的消息内容
        this->declare_parameter<std::string>("serial_port0", "/dev/ttyUSB0");
        this->declare_parameter<int>("baud_rate0", 115200);
        this->declare_parameter<std::string>("serial_port1", "/dev/ttyUSB1");
        this->declare_parameter<int>("baud_rate1", 115200);
        this->declare_parameter<double>("wheel_radius", 0.1); //轮子半径，单位米
        this->declare_parameter<double>("wheel_base", 0.6); //轮子间距，单位米
        this->declare_parameter<double>("wheel_track", 1.2);
        for (int i = 0; i < 8; ++i) {
            this->declare_parameter<int>("motor" + std::to_string(i) + "_id", 0);
            this->declare_parameter<int>("motor" + std::to_string(i) + "_delay", 100);
            this->declare_parameter<int>("motor" + std::to_string(i) + "_direction", 1); // 1=正转 0=反转
        }
        for (int i = 0; i < 4; ++i) {
           this->declare_parameter<int>("bianma" + std::to_string(i) + "_id", 0);
           this->declare_parameter<int>("bianma" + std::to_string(i) + "_delay", 100);
        }
        for (int i = 0; i < 8; ++i) {
            this->declare_parameter<int>("sensor" + std::to_string(i) + "_id", 0);
            this->declare_parameter<int>("sensor" + std::to_string(i) + "_delay", 100);
        }
        for (int i = 0; i < 4; ++i) {
            this->declare_parameter<int>("bianma" + std::to_string(i) + "_zero", 0);
        }
        this->declare_parameter<int>("yaokong_channel", 0);
       
    }
    // 获取所有参数
    void get_parameters() {
        this->get_parameter("run_mode", run_mode); //0=正常运行模式 1=查看接收到的消息数量 2=查看接收到的消息内容
        this->get_parameter("serial_port0", motor_config.serial_port0_);
        this->get_parameter("baud_rate0", motor_config.baud_rate0_);
        this->get_parameter("serial_port1", motor_config.serial_port1_);
        this->get_parameter("baud_rate1", motor_config.baud_rate1_);
        this->get_parameter("wheel_radius", motor_config.wheel_radius);
        this->get_parameter("wheel_base", motor_config.wheel_base);
        this->get_parameter("wheel_track", motor_config.wheel_track);
        for (int i = 0; i < 8; ++i) {
            this->get_parameter("motor" + std::to_string(i) + "_id", motor_config.motor_drv[i].Id);
            this->get_parameter("motor" + std::to_string(i) + "_delay", motor_config.motor_drv[i].delay);
            this->get_parameter("motor" + std::to_string(i) + "_direction", motor_config.motor_drv[i].direction);
        }
        for(int i = 0; i < 4; ++i) {
            this->get_parameter("bianma" + std::to_string(i) + "_id", motor_config.bianma_drv[i].Id);
            this->get_parameter("bianma" + std::to_string(i) + "_delay", motor_config.bianma_drv[i].delay);
        }
        
        for(int i = 0; i < 8; ++i) {
            this->get_parameter("sensor" + std::to_string(i) + "_id", motor_config.sensor_drv[i].Id);
            this->get_parameter("sensor" + std::to_string(i) + "_delay", motor_config.sensor_drv[i].delay);
        }
        for(int i = 0; i < 4; ++i) {
            this->get_parameter("bianma" + std::to_string(i) + "_zero", motor_config.bianma_zero[i]);
        }
        this->get_parameter("yaokong_channel", motor_config.yaokong_channel);
    }
    Motor_Config motor_config;
    rclcpp::TimerBase::SharedPtr timer_;
    MotorControl *motor_control0_ = nullptr;
    int run_mode = 0; // 0=正常运行模式 1=查看接收到的消息数量 2=查看接收到的消息内容
};

int main(int argc, char * argv[])
{  
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MotorDriverNode>());
    rclcpp::shutdown();
    return 0;
}


